#!/usr/bin/env python
'''
Created 2013-05-23

@author: Tamer Saadeh

Based on code by Dr. Rainer Hessmer from Jan-2011
'''

# TODO: check if all of these are actually used/needed

import roslib; roslib.load_manifest('playground')
import fnmatch
import rospy
import tf
import sys
import time

from time import gmtime, strftime

from std_msgs.msg import String
from playground.srv import *

from SerialData import SerialData

# TODO: probably needs to be made clearer
def auto_detect_serial_unix(preferred_list=['*']):
    '''
    Try to auto-detect serial devices on linux
    '''

    import glob
    glist = glob.glob('/dev/ttyUSB*')
    ret = []

    # try preferred ones first
    for d in glist:
	for preferred in preferred_list:
	    if fnmatch.fnmatch(d, preferred):
	        ret.append(d)
    if len(ret) > 0:
	return ret
    # now the rest
    for d in glist:
	ret.append(d)
    return ret

class Arduino(object):
	'''
	Class for communicating with an Arduino board using the SerialData class, and forwarding it to ROS.
	'''

	def _HandleReceivedLine(self, line):
		'''
		Handles data recieved from Arduino and forwards to a ROS publisher.
		line: The line recieved from Arduino.
		'''

		rospy.logdebug("Arduino: Handling received line.")

		# Line number coutner
		self._Counter = self._Counter + 1

		# Sets the time variables in UTC
		self._tmstmp = strftime("%a, %d %b %Y %H:%M:%S +0000", gmtime())

		# Send line recieved from Arduino to a ROS publisher
		self._Publisher.publish(String(str(self._Counter) + " " + self._tmstmp + " ; " + line))

		if (len(line) > 0):
			lineParts = line.split('\t')


    	def handleSendCommands(self, command):
		'''
		Handles the data to send to Arduino.
		command: The command to send to Arduino.
		'''

		rospy.logdebug("Arduino: Sending commands: '" + command.message + "'.")
		self._SerialData.Write(command.message)
		return command.message


	def __init__(self, device="/dev/ttyUSB0", baudrate=115200):
		'''
		Initializes the receiver class. 
		device: The serial device to listen to, defaults to /dev/ttyUSB0.
		baudrate: Baud rate for the serial communication, defaults to 115200.
		'''

        	rospy.logdebug("Arduino: Initializing.")

		self._Counter = 0
		self._tmstmp = strftime("%a, %d %b %Y %H:%M:%S +0000", gmtime())

		rospy.init_node('arduino')

		port = rospy.get_param("~port", "/dev/ttyUSB0")
		baudRate = int(rospy.get_param("~baudRate", 115200))
		rospy.loginfo("Arduino: Starting with serial port: '" + port + "', baud rate: " + str(baudRate) + ".")

		# Publish to /serial
		self._Publisher = rospy.Publisher('serial',String)

		self.s = rospy.Service('SendCommands', SendCommands, self.handleSendCommands)

		rospy.loginfo("Arduino: Ready to send commands.")

        	rospy.logdebug("Arduino: Initialized.")


    	def Start(self, port):
		'''
		Wrapper that starts the communication link with Arduino.
		'''

		#rospy.init_node('arduino')
		# Publish to /serial
		#self._Publisher = rospy.Publisher('serial',String)
		#self.s = rospy.Service('SendCommands', SendCommands, self.handleSendCommands)
		#rospy.loginfo("Arduino: Ready to send commands.")
        	#rospy.logdebug("Arduino: Initialized.")

		rospy.loginfo("Arduino: Trying '" + port + "'.")
		baudRate = int(rospy.get_param("~baudRate", 115200))

		self._SerialData = SerialData(port, baudRate, self._HandleReceivedLine)

		rospy.loginfo("Arduino: Starting.")
		self._SerialData.Start()
        	self._Stopped = False
		rospy.loginfo("Arduino: Started.")


	def Stop(self):
		'''
		Wrapper that stops the communication link with Arduino.
		'''

		rospy.logdebug("Arduino: Stopping.")
		self._SerialData.Stop()
        	self._Stopped = True
		rospy.logdebug("Arduino: Stopped.")


if __name__ == '__main__':
	arduino = Arduino()
	arduino._Stopped = False
    	#rospy.logdebug("Arduino: Number of available devices: " + rospy.loginfo(len(available_ports)) + ".")

    	while not rospy.is_shutdown(): #arduino._Stopped:
		available_devices = auto_detect_serial_unix()
		rospy.loginfo("Arduino: Number of available devices: " + str(len(available_devices)) )

		for device in available_devices:
	    		try:
        			rospy.loginfo("Arduino: Device: '" + device + "'")

				# Start Arduino
				arduino.Start(device)
				try:
					arduino._SerialData._Listen()
				except:
					rospy.loginfo("Arduino: starting problem")	
					break
				#rospy.spin()

				# This should never be true unless we call arduino.stop()
				#if arduino._SerialData._Running == False:
					#rospy.loginfo("Arduino: Should be reached as we're not threading anymore.")
					#available_ports = auto_detect_serial_unix()
					#port =  available_ports[0]
					#rospy.loginfo("PORTE:"+port)
					#arduino.reStart(port)
					#rospy.spin()

		    	except rospy.ROSInterruptException:
				arduino.Stop()
				break

	    		except:
	        		rospy.loginfo("Arduino: Link to Arduino was interrupted.")	

		time.sleep(1)
